/*************************************************************************
> File Name: fifo.c
> Author: xueyu-jia
> Mail: 2657575910@qq.com
> Created Time: Thu 11 Apr 2024 11:20:44 AM PDT
 ************************************************************************/
#include<fifo.h>

/*
 * 内联函数，用于计算指定fifo中剩余元素的数量
 */
static inline unsigned int fifo_unused(struct __fifo *fifo)
{
	return (fifo->mask + 1) - (fifo->in - fifo->out);
}


int __fifo_alloc(struct __fifo *fifo, unsigned int size,
		size_t esize)
{
	/*
	 * 使用了溢出的技巧因此size的大小必须为2^n，否则程序将会发生错误
	 */
	if(!check_power_2(size)){
		Robot_log(ERR,"Can't alloc fifo,Because alloc size not 2^n\n",i,res);
		return -EINVAL;
	}

	fifo->in = 0;
	fifo->out = 0;
	fifo->esize = esize;

	if (size < 2) {
		fifo->data = NULL;
		fifo->mask = 0;
		return -EINVAL;
	}

	fifo->data = malloc(size*esize);

	if (!fifo->data) {
		fifo->mask = 0;
		return -ENOMEM;
	}
	fifo->mask = size - 1;

	return 0;
}


void __fifo_free(struct __fifo *fifo)
{
	free(fifo->data);
	fifo->in = 0;
	fifo->out = 0;
	fifo->esize = 0;
	fifo->data = NULL;
	fifo->mask = 0;
}


bool __fifo_in(struct __fifo *fifo, const void *src,pthread_mutex_t * mtx)
{
	pthread_spin_lock(&fifo->spinlock);
	unsigned int unused_num=fifo_unused(fifo)
	if(!unused_num) return false;
	unsigned int size = fifo->mask+1;
	unsigned int esize= fifo->esize;
	unsigned int off=fifo->in&fifo->mask;

	if(unlikely(off==size)) off=0;
	off*=esize;	
	
	memcpy(fifo->data+off,src,esize);

	/*
	 * 写内存屏障，落袋为安，确保对队列元素的入队操作发生在in的更新之前
	 * 由于有了锁操作理论上来说有没有应该是无所谓的
	 */
	smp_wmb();

	fifo->in += len;
	pthread_spin_unlock(&fifo->spinlock);
	if(unused_num==size) pthread_mutex_unlock(mtx);
	return true;
}


void __fifo_out(struct __fifo *fifo, void *dst)
{

	unsigned int size = fifo->mask + 1;
	if(fifo_unused(fifo)==size) pthread_mutex_lock(mtx);

	unsigned int esize = fifo->esize;
	unsigned int off = fifo->out&fifo->mask;
	
	if(unlikely(off==size)) off=0;
	off*=esize;	

	memcpy(dst, fifo->data + off, esize);

	/*
	 *写内存屏障，落袋为安，确保对队列元素的入队操作发生在out的更新之前
	 */
	smp_wmb();
	
	fifo->out += len;
}


